import vue from '../main'
import store from '../store/index';
import { Notification  } from 'element-ui';
import { OBJ, viewer } from './init'
import { showTrajectoryAdd } from "./trajectory"
import { checkStr, quaternionToYaw } from "./methods"
import { isNameUsedtraject } from './trajectory'

/**
 * show way points
 */
export function showWayPoints() {
    return new Promise(function (resolve, reject) {
        OBJ.topic.wpTopic.subscribe((message) => {
            OBJ.waypointsMessage = message;
            if (OBJ.waypointsStage) {
                for (var key in OBJ.waypointsStage) {
                    if (OBJ.waypointsStage.hasOwnProperty(key)) {
                        var waypointsContainers = OBJ.waypointsStage[key];
                        for (var i = 0; i < waypointsContainers.length; i++) {
                            viewer.scene.removeChild(waypointsContainers[i]);
                        }
                    }
                }
            }

            OBJ.wayPointsMsg = message;
            var waypointsArr = [];

            //获取到站点信息之后赋给waypointsMsg，渲染站点，添加事件
            for (var i = 0; i < message.waypoints.length; i++) {
                var waypoint = message.waypoints[i];
                var wpInfo = JSON.parse(waypoint.header.frame_id);
                var waypointType = wpInfo.type;

                //如果不是目标站点，直接跳过
                switch(waypointType){
                    case "goal":
                        break;
                        case "goal":
                            break;
                        case "publisher":
                            break;
                        case "call":
                            break;
                        case "homing_control":
                            break;
                        case "std_msgs/Header":
                            break;
                    default:
                        continue;
                        break;
                }
                
                var wpContainer = new ROS2D.NavigationArrow({
                    size: 10,
                    strokeSize: 1,
                    fillColor: createjs.Graphics.getRGB(63, 134, 254, 0.9),
                    pulse: false
                });
                wpContainer.x = waypoint.pose.position.x;
                wpContainer.y = -waypoint.pose.position.y;
                wpContainer.scaleX = 1.0 / viewer.scene.scaleX;
                wpContainer.scaleY = 1.0 / viewer.scene.scaleY;
                // change the angle
                wpContainer.rotation = viewer.scene.rosQuaternionToGlobalTheta(waypoint.pose.orientation);

                wpContainer.name = waypoint.name; // bind name
                waypointsArr.push(wpContainer)
                // TODO: handle wp click
                //点击站点导航到站点
                wpContainer.on('click', waypointClick);
                viewer.scene.addChild(wpContainer)
            }
            OBJ.waypointsStage = waypointsArr;
            store.commit('stationList', waypointsArr);
            resolve('wayPoints success');
        });
    })
}

/**
 * 显示站点列表
 * flag :true显示  :false隐藏
 * **/
export function showWaypointList(flag){
    if(flag){
        store.commit('switchWaypointList', true);
    }else{
        store.commit('switchWaypointList', false);
    }
}

/**
 * 点击添加站点
 * 显示/隐藏 站点表单和站点列表
 * 计算小车定位和姿态
 * 缓存小车姿态
 * **/
export function getRobotPose() {
    //订阅robot pose
    if (Object.keys(OBJ.robotPoseMsg).length !== 0) {
        let yawRad = quaternionToYaw(OBJ.robotPoseMsg.orientation);
        let yawDeg = (yawRad * 180 / Math.PI).toFixed(8);
        let infos = {
            positionX: OBJ.robotPoseMsg.position.x.toFixed(5),
            positionY: OBJ.robotPoseMsg.position.y.toFixed(5),
            positionZ: OBJ.robotPoseMsg.position.z.toFixed(5),
            orientationX: OBJ.robotPoseMsg.orientation.x.toFixed(5),
            orientationY: OBJ.robotPoseMsg.orientation.y.toFixed(5),
            orientationZ: OBJ.robotPoseMsg.orientation.z.toFixed(5),
            orientationW: OBJ.robotPoseMsg.orientation.w.toFixed(5),
            yaw: yawDeg
        }
        store.commit('setRobotPosition', infos);
        return true;
    } else {
        Notification({
            title: vue.$t("titles.error"),
            message: vue.$t("messages.robotPoseUnavailable") + "，建议重启系统",
            type: 'error',
            position: 'top-left',
            offset: 100
        });
        return false;
    }
};

/**
 * 显示新建站点
 * flag :true显示  :false隐藏
 * 如果用户引导程序未结束，显示新建站点
 * **/
export function showWaypoint(flag=false){
    let guide = window.localStorage.getItem('guide');
    if(flag){
        store.commit('switchAddStation',true);
        if(!guide){
            store.commit("guideIndex", 2);
        }
    }else{
        store.commit('switchAddStation', false);
    }
}


/**
 * 确定删除站点topic
 * **/
export function deleteWaypoint(fullName) {
    var waypoint = getWaypointByName(fullName);//站点对象
    var msg = new ROSLIB.Message(waypoint);
    console.log(`delete Waypoint Name:${fullName}`)
    OBJ.topic.delWaypointTopic.publish(msg);
    viewer.scene.removeChild(waypoint);

    Notification({
        title: vue.$t("titles.success"),
        message:vue.$t("messages.WaypointDelSuccess"),
        type: 'success',
        position: 'top-left',
        offset: 100
    });
};

/**
 * 点击站点图标或名字
 * 显示站点详情、遮罩，隐藏站点列表
 * 匹配对应的站点，有匹配则赋值
 * **/
export function showWaypointDetail(flag = true, fullName) {
    if(flag){
        var waypoint = getWaypointByName(fullName);//站点信息
        console.log('[INFO]:' + JSON.stringify(waypoint))
        var wpInfo = JSON.parse(waypoint.header.frame_id);
        if (waypoint) {
            store.commit('wayPointMsg', waypoint);
            console.log(`[INFO]waypoint:\n-type: ${wpInfo.type};\n-name: ${waypoint.name}.`);
        } else {
            Notification({
                title: vue.$t("titles.error"),
                message: vue.$t("messages.notFindWaypoint") + fullName,
                type: 'error',
                position: 'top-left',
                offset: 100
            });
            console.warn(`[WARN]Can not find waypoint ${fullName}`);
            return
        }
        store.commit('switchWaypointDetail', true);
    }else{
        store.commit('switchWaypointDetail', false);
    }
};

/**
 * 通过名称找到对应站点数据
 * **/
export function getWaypointByName(name) {
    for (var i = 0; i < OBJ.wayPointsMsg.waypoints.length; i++) {
        var waypoint = OBJ.wayPointsMsg.waypoints[i];
        if (waypoint.name === name) {
            return waypoint;
        }
    }
}


/**
 * 新建站点-提交
 * stationName:站点名称，stationType：站点类型，frameId
 * closeEnough：范围，timeout：超时，stationMode：站点模式
 * **/
export function submitAddWaypoint(infos) {
    let stationName = infos.stationName;
    let stationType = infos.type;
    let frameId = infos.frameId;
    // let closeEnough = parseFloat(infos.scope);
    // let timeout = parseFloat(infos.timeout);
    let stationMode = infos.mode;
    if (!checkStr(stationName)) {
        Notification({
            title: vue.$t("titles.warning"),
            message: vue.$t("messages.waypointNotValid"),
            type: 'warning',
            position: 'top-left',
            offset: 100
        });
        return;
    }
    if (isNameUsedWaypoints(stationName)) {
        Notification({
            title: vue.$t("titles.warning"),
            message: vue.$t("messages.waypointExists"),
            type: 'warning',
            position: 'top-left',
            offset: 100
        });
        return;
    }

    if (isNameUsedtraject(stationName)) {
        Notification({
            title: vue.$t("titles.warning"),
            message: vue.$t("messages.waypointWithTrajectoryExists"),
            type: 'warning',
            position: 'top-left',
            offset: 100
        });
        return;
    }

    if (!frameId) {
        if (stationType === 'initial_pose' || stationType === 'goal') {
            Notification({
                title: vue.$t("titles.warning"),
                message: vue.$t("messages.frame_id"),
                type: 'warning',
                position: 'top-left',
                offset: 100
            });
            return;
        }
    }

    //如果是声音播放类型，切换站点模式
    if (stationType === 'sound_play') {
        stationMode = infos.voiceMode;
    }

    //如果是call类型，切换站点模式
    if (stationMode == 'call') {
        stationMode = infos.callMode;
    }

    //如果是goto类型，切换站点模式
    if (stationMode == 'goto') {
        stationMode = infos.gotoMode;
    }

    //判断站点模式是否为空
    if (!checkStr(stationMode)) {
        Notification({
            title: vue.$t("titles.error"),
            message: vue.$t("messages.waypointModeNotValid"),
            type: 'error',
            position: 'top-left',
            offset: 100
        });
        return;
    }

    //引导流程为站点
    window.localStorage.setItem('guideName', 'waypoint');

    let pose = {
        position: OBJ.robotPoseMsg.position,
        orientation: OBJ.robotPoseMsg.orientation
    };

    let waypointInfo = {
        stationName,
        stationType,
        stationMode,
        frameId,
        // closeEnough,
        // timeout,
        pose
    }

    // failure_mode: waypointInfo.stationMode,
    let frameIdStr = {
        frame_id: waypointInfo.frameId,
        type: waypointInfo.stationType,
        close_enough: 0.0,
        goal_timeout: 0.0,
        failure_mode: waypointInfo.stationMode,
    }
    var msg = new ROSLIB.Message({
        header: {
            frame_id: JSON.stringify(frameIdStr)
        },
        name: waypointInfo.stationName,
        pose: waypointInfo.pose,
    });
    OBJ.topic.addWaypointTopic.publish(msg);
    console.log(msg)
    Notification({
        title: vue.$t("titles.success"),
        message: vue.$t("messages.waypointAddSuccess"),
        type: 'success',
        position: 'top-left',
        offset: 100
    });
    showWaypoint(false);
    //显示新建轨迹模态框
    let guide = window.localStorage.getItem('guide');
    if(!guide){
        showTrajectoryAdd(true);
    }
};

export function showWayPoints2(){
    var flag = false;
    if (OBJ.waypointsStage) {
        if (OBJ.waypointsStage) {
            for (var key in OBJ.waypointsStage) {
                var waypointsContainers = OBJ.waypointsStage[key];
                for (var i = 0; i < waypointsContainers.length; i++) {
                    viewer.scene.removeChild(waypointsContainers[i]);
                    flag = true;
                }
            }
            setTimeout(function(){
                if(flag){
                    var waypointContainers = {
                        goal: [],
                    };
                    for(var j = 0; j < OBJ.waypointsMessage.waypoints.length; j++){
                        //获取到站点信息之后赋给waypointsMsg，渲染站点，添加事件
                        var waypoint = OBJ.waypointsMessage.waypoints[j];
                        var wpInfo = JSON.parse(waypoint.header.frame_id);
                        var waypointType = wpInfo.type;
            
                        //如果不是目标站点，直接跳过
                        if (waypointType !== 'goal') {
                            continue;
                        }
                        var wpContainer = new ROS2D.NavigationArrow({
                            size: 10,
                            strokeSize: 1,
                            fillColor: createjs.Graphics.getRGB(63, 134, 254, 0.9),
                            pulse: false
                        });
                        wpContainer.x = waypoint.pose.position.x;
                        wpContainer.y = -waypoint.pose.position.y;
                        wpContainer.scaleX = 1.0 / viewer.scene.scaleX;
                        wpContainer.scaleY = 1.0 / viewer.scene.scaleY;
                        // change the angle
                        wpContainer.rotation = viewer.scene.rosQuaternionToGlobalTheta(waypoint.pose.orientation);
            
                        wpContainer.name = waypoint.name; // bind name
                        waypointContainers[waypointType].push(wpContainer);
                        // TODO: handle wp click
                        //点击站点导航到站点
                        wpContainer.on('click', waypointClick);
                        viewer.scene.addChild(wpContainer)
                        OBJ.waypointsStage = waypointContainers;
                        store.commit('stationList', waypointContainers.goal);
                    }
                }
            }, 200) 
        }  
    }
}

//click way point
function waypointClick(event) {
    if(!store.state.userRole){
        return
    }
    showWaypointDetail(true, event.currentTarget.name);
}

/**
 * 站点名字是否重复
 * **/
export function isNameUsedWaypoints(name) {
    if (OBJ.wayPointsMsg) {
        for (var i = 0; i < OBJ.wayPointsMsg.waypoints.length; i++) {
            if (OBJ.wayPointsMsg.waypoints[i].name === name) {
                return true;
            }
        }
    }
    return false;
};